
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <stdbool.h>

#include <log_util.h>
#include <memory_util.h>

#include <ebex_cfg.h>
#include <ebex_filter_cfg.h>
#include <ebex_genetic.h>
#include <ebex_genetic_chromosome.h>
#include <ebex_model.h>
#include <data_file.h>

#include <ebex_bootstrap.h>
#include <ebex_steady_state.h>

int shutdown_now = 0;

/// Signal handler called when we get a hup, int or term
static void shutdown_filter(int signo)
{
	elog(log_level_normal, "Caught signal %i", signo);

	shutdown_now = 1;

	/// restore default handler
	signal(signo, SIG_DFL);
}

static int ebex_filter_add_data_sources(ebex_model_t *new_model)
{
	data_file_t *new_file;
	data_source_t *new_source;

	for (size_t i = 0; i < filter_config.num_sc; i++)
	{
		new_source = ebex_data_new_source(2 * filter_config.bootstrap.num_sc_images);

		for (size_t j = 0; j < 3; j++)
		{
			if (!(new_file = ebex_data_add_datafile(new_source, filter_config.sc_files[i][j].filename, filter_config.sc_files[i][j].type, filter_config.sc_files[i][j].offset, false)))
			{
				dief("Could not add %s to the datasource", filter_config.sc_files[i][j].filename);
				return EBEX_FAIL;
			}

			memcpy(&new_file->conf, &filter_config.sc_files[i][j].conf, sizeof(new_file->conf));
		}
		if (!(new_file = ebex_data_set_timefile(new_source, filter_config.sc_files[i][3].filename, filter_config.sc_files[i][3].type, filter_config.sc_files[i][3].offset, false)))
		{
			dief("Could not set %s as the timestream", filter_config.sc_files[i][3].filename);
			return EBEX_FAIL;
		}
		memcpy(&new_file->conf, &filter_config.sc_files[i][3].conf, sizeof(new_file->conf));

		/**
		 * Now for the uncertainty streams
		 */
		for (size_t j = 4; j < 7; j++)
		{
			if (!(new_file = ebex_data_add_datafile(new_source, filter_config.sc_files[i][j].filename, filter_config.sc_files[i][j].type, filter_config.sc_files[i][j].offset, false)))
			{
				dief("Could not add %s to the datasource", filter_config.sc_files[i][j].filename);
				return EBEX_FAIL;
			}

			memcpy(&new_file->conf, &filter_config.sc_files[i][j].conf, sizeof(new_file->conf));
		}

		ebex_gyro_sc_add_sc_src(new_model->data, new_source);

		memcpy(&new_model->sc[i].alignment_noise, &filter_config.sc[i].alignment_noise, sizeof(random_sample_t));
		ebex_quat_from_euler(&new_model->sc[i].alignment, &filter_config.sc[i].rpy, rotation_order_XYZ);
		new_model->sc[i].mutation_likelihood = filter_config.sc[i].mutation_likelihood;
	}


	for (size_t box = 0; box < filter_config.num_gyros; box++)
	{
		/// Arbitrary 120 seconds worth of gyro data in the initial buffer
		new_source = ebex_data_new_source(120 * (size_t)ceil(filter_config.gyro_rate));

		for (size_t gyro = 0; gyro < 3; gyro++)
		{
			if (!(new_file = ebex_data_add_datafile(new_source, filter_config.gyro_files[box][gyro].filename, filter_config.gyro_files[box][gyro].type, filter_config.gyro_files[box][gyro].offset, false)))
			{
				dief("Could not add %s to the datasource", filter_config.gyro_files[box][gyro].filename);
				return EBEX_FAIL;
			}
			memcpy(&new_file->conf, &filter_config.gyro_files[box][gyro].conf, sizeof(new_file->conf));

			new_model->gyrobox[box].gyro[gyro].is_inverted = filter_config.gyro_box[box].gyro[gyro].is_inverted;
			new_model->gyrobox[box].gyro[gyro].name = e_strdup(filter_config.gyro_box[box].gyro[gyro].name);
			memcpy(&new_model->gyrobox[box].gyro[gyro].param, &filter_config.gyro_box[box].gyro[gyro].param, sizeof(gyro_param_t));
		}
		memcpy(&new_model->gyrobox[box].alignment_noise, &filter_config.gyro_box[box].alignment_noise, sizeof(random_sample_t));
		ebex_quat_from_euler(&new_model->gyrobox[box].alignment, &filter_config.gyro_box[box].rpy, rotation_order_XYZ);
		new_model->gyrobox[box].mutation_likelihood = filter_config.gyro_box[box].mutation_likelihood;

		if (!(new_file = ebex_data_set_timefile(new_source, filter_config.gyro_files[box][3].filename, filter_config.gyro_files[box][3].type, filter_config.gyro_files[box][3].offset, false)))
		{
			dief("Could not set %s as the timestream", filter_config.gyro_files[box][3].filename);
			return EBEX_FAIL;
		}
		memcpy(&new_file->conf, &filter_config.gyro_files[box][3].conf, sizeof(new_file->conf));

		ebex_gyro_sc_add_gyrobox_src(new_model->data, new_source);
	}

	if (filter_config.output_parent)
	{
		new_source = ebex_data_new_source(120 * (size_t)ceil(filter_config.gyro_rate));

		for (size_t i = 0; i < 3; i++)
		{
			if (!(new_file = ebex_data_add_datafile(new_source, filter_config.parent_files[i].filename, filter_config.parent_files[i].type, filter_config.parent_files[i].offset, false)))
			{
				dief("Could not add %s to the datasource", filter_config.parent_files[i].filename);
				return EBEX_FAIL;
			}
			memcpy(&new_file->conf, &filter_config.parent_files[i].conf, sizeof(new_file->conf));
		}
		if (!(new_file = ebex_data_set_timefile(new_source, filter_config.gyro_files[0][3].filename, filter_config.gyro_files[0][3].type, filter_config.gyro_files[0][3].offset, false)))
		{
			dief("Could not set %s as the timestream", filter_config.gyro_files[0][3].filename);
			return EBEX_FAIL;
		}
		memcpy(&new_file->conf, &filter_config.gyro_files[0][3].conf, sizeof(new_file->conf));

		ebex_gyro_sc_add_parent_src(new_model->data, new_source);
	}

	return EBEX_SUCCESS;
}

static void ebex_filter_store_parameters(population_t *m_pop)
{
	ebex_model_t *model = m_pop->data;
	organism_t *mean_entity = ebex_genetic_get_mean_organism(m_pop);

	for (size_t box = 0; box < model->num_gyroboxes; box++)
	{
		size_t gyrobox_chromosome = box + num_gyro_params;
		for (size_t chromosome = 0; chromosome < num_gyro_params; chromosome++)
		{
			for (size_t k = 0; k < 3; k++)
			{
				size_t offset = box * 3 + k;
				model->gyrobox[box].gyro[k].param.value.array[chromosome] = mean_entity->chromosome[chromosome].dbl_allele[offset];
			}
		}
		ebex_quat_copy(&model->gyrobox[box].alignment, &mean_entity->chromosome[gyrobox_chromosome].q_allele[box]);
	}

	for (size_t i = 0; i < model->num_sc; i++)
	{
		size_t sc_chromosome = i + num_gyro_params + model->num_gyroboxes;
		ebex_quat_copy(&model->sc[i].alignment, &mean_entity->chromosome[sc_chromosome].q_allele[i]);
	}
}

int main(int argc, char **argv)
{
	ebex_model_t *new_model;
	population_t *param_pop;

	signal(SIGHUP, shutdown_filter);
	signal(SIGINT, shutdown_filter);
	signal(SIGTERM, shutdown_filter);

	log_init(log_level_normal, NULL, true);
	ebex_genetic_init();
	ebex_filter_cfg_register();
	if (ebex_cfg_process(argc, argv, basename(argv[0]),NULL) < 0)
		die("Could not load configuration file");

	if (filter_config.num_gyros < 1)
	{
		die("Need at least 1 gyro box to run");
	}
	if (filter_config.num_sc < 1)
	{
		die("Need at least 1 star camera to run");
	}
	if (filter_config.gyro_rate <= DBL_TINY)
	{
		dief("Invalid Gyro sample rate %f", filter_config.gyro_rate);
	}



	new_model = ebex_model_new(filter_config.num_gyros, filter_config.num_sc, filter_config.steady_state.max_segments);

	new_model->parameters = &filter_config;
	new_model->data = ebex_gyro_sc_new();

	ebex_cfg_cleanup();

	if (ebex_filter_add_data_sources(new_model) != EBEX_SUCCESS) return EXIT_FAILURE;

	param_pop = genetic_pop_new(filter_config.bootstrap.num_segments, filter_config.num_gyros + filter_config.num_sc + num_gyro_params );
	param_pop->chromosome_constructor = ebex_gyro_sc_generate_chromosome;
	param_pop->chromosome_destructor = ebex_genetic_chromosome_free;
	param_pop->data = new_model;

	for (size_t i = 0; i < filter_config.bootstrap.num_segments && !shutdown_now; i++)
	{
		new_model->data->produce(new_model->data, filter_config.bootstrap.num_sc_images);
		ebex_state_bootstrap(new_model, param_pop);
		new_model->data->produce(new_model->data, filter_config.bootstrap.sc_segment_jump);
		new_model->data->consume(new_model->data, filter_config.bootstrap.num_sc_images + filter_config.bootstrap.sc_segment_jump);
	}

	ebex_filter_store_parameters(param_pop);
	ebex_state_print_stats(param_pop);
	genetic_close(param_pop);


	if (!shutdown_now)
	{
		new_model->data->reset(new_model->data);
		ebex_steady_state(new_model);
	}


	return EXIT_SUCCESS;
}

